﻿/*
 * Copyright (c) 2014 The Plasix Authors
 * Plasix (Px) Library
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in
 * all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
 * THE SOFTWARE.
 */
#ifndef PX_VECTOR_3_H
#define PX_VECTOR_3_H

#include <iostream>
#include <cmath>
#include <type_traits>

/* Plasix Library Includes */
#include <pxMath.h>

namespace Px {
namespace Math {

/* Forward Declaration for Templated Friends */
template <typename Real>
class Vector3;

/*
* Templated Friends
* See: http://www.parashift.com/c++-faq-lite/template-friends.html
*/
template <typename Real>
Vector3<Real> operator + (const Vector3<Real>& lhs, const Vector3<Real>& rhs);

template <typename Real>
Vector3<Real> operator + (const Vector3<Real>& lhs, const Vector3<Real>& rhs);

template <typename Real>
Vector3<Real> operator - (const Vector3<Real>& lhs, const Vector3<Real>& rhs);
    
template <typename Real>
Vector3<Real> operator - (const Vector3<Real>& vector);

template <typename Real>
Vector3<Real> operator * (const Vector3<Real>& vector, Real scalar);

template <typename Real>
Vector3<Real> operator * (Real scalar, const Vector3<Real>& vector);

template <typename Real>
Vector3<Real> operator / (const Vector3<Real>& vector, Real scalar);

template <typename Real>
std::ostream& operator << (std::ostream& out, const Vector3<Real>& vector);

template <typename Real>
std::istream& operator >> (std::istream& in, Vector3<Real>& vector);

/* 
 * Vector3: Representation of any numerical vector in three dimensions.
 *
 * (x, y, z);
 * 
 * Implements the mathematical defintion of a Vector3. Do not perform long 
 * "temp-chains" using overloaded operators. This implementaton does not utilize 
 * template meta programming.
 * See: http://www.flipcode.com/archives/Faster_Vector_Math_Using_Templates.shtml
 *
 * This implementation also foregoes vectorization (SSE).
 * This implementation is aimed at a flexibility while preserving 
 * understandability and complete modularity.
 */
template <typename Real>
class Vector3 {
    /*
     * Static assertion to prevent misuse of the template type. This 
     * implementation is designed to only support numerical types (integral or 
     * floating point representations). If any other type is provided, this 
     * error will be raised.
     */
    static_assert(std::is_integral<Real>::value || std::is_floating_point<Real>::value, "[Px:Vector3:type] Error: Vector3 Real type must be an integral or floating point numerical representation.");

    /* Output separator for formatting vector components. */
    static const char SEPARATOR = ' ';

public:
    Vector3();
    Vector3(const Vector3<Real>& vector);
    Vector3(Real uniform);
    Vector3(Real a[3]);
    Vector3(Real x, Real y, Real z);
    Vector3(const Vector3<Real>& from, const Vector3<Real>& to);
    virtual ~Vector3();

    void setX(Real x);
    void setY(Real y);
    void setZ(Real z);

    /* Const and non-const component member access. */
    Real getX() const;
    Real getY() const;
    Real getZ() const;
    Real& x();
    Real& y();
    Real& z();

    /* 
     * Returns the angle between this vector and the provided vector.
     * (see Angle).
     */
    double angle(const Vector3<Real>& vector);

    /* Returns the length/magnitude/norm of this vector. */
    double magnitude() const;
    double norm() const;
    double length() const;
    double lengthSquared() const;

    /* Returns the distance between this vector and the provided vector. */
    double distance(const Vector3<Real>& vector);
    double distanceSquared(const Vector3<Real>& vector);

    void scale(Real scalar);
    void multiply(Real scalar);
    void multiply(const Vector3<Real>& vector);
    void add(const Vector3<Real>& vector);
    void subtract(const Vector3<Real>& vector);

    /* 
     * Normalizes this vector to be unit length (1.0). If an integral datatype
     * is used then the truncation will limit the resolution of the result
     * and will not represent a mathematical unit vector.
     */
    void normalize();

    /* Inverses each component of this vector: x *= -1, y *= -1, z *= -1 */
    void inverse();

    /* Sets each component of this vector to zero: (0, 0, 0) */
    void zero();
    bool isZero(Real epsilon = 1.0e-4) const;

    /* 
     * If this vector is equal to the provided vector then this function will
     * return true; otherwise it will return false (equivalent to operator == )
     */
    bool equal(const Vector3<Real>& vector);

    /* 
     * Cast this vector (with numerical representation Real) to a vector with
     * the numerical representation RealCastType>
     */
    template <typename RealCastType>
    Vector3<RealCastType> cast();

    /* 
     * Compares this vector with the provided vector. If each of the components
     * is within the epsilon error interval then they will be considered
     * equivalent. Functionality is related only to floating point Real types.
     */
    bool equivalent(const Vector3<Real>& vector, double epsilon = 1.0E-6);

    /* Set the values of the components independently. */
    void set(const Vector3<Real>& data);
    void set(Real x, Real y, Real z);
    void set(Axis axis, Real& value);

    /* Set the length of this vector to the provided scalar */
    void setLength(Real length);

    /* Retrieve the value of this vectors components. */
    void get(Vector3<Real>& data) const;
    void get(Real& x, Real& y, Real& z) const;
    void get(Axis axis, Real& value) const;
    Real get(Axis axis) const;
    
    /* 
     * Returns the dot product between this vector and the provided vector.
     * (see: Dot).
     */
    double dot(const Vector3<Real>& vector) const;
    double innerProduct(const Vector3<Real>& vector) const;

    /* 
     * Returns the cross product between this vector and the provided vector.
     * The resulting vector is not normalized (see: Cross).
     */
    Vector3<Real> cross(const Vector3<Real>& vector) const;
    
    /* Returns a normalized copy of this vector. */
    Vector3<Real> normalized() const;
    
    /* 
     * Calculates the reflection vector based on the provided normal. 
     * (see: static functions Reflect and ReflectDir) 
     */
    Vector3<Real> reflect(const Vector3<Real>& normal, bool backcull = false);
    Vector3<Real> reflectDir(const Vector3<Real>& normal, bool backcull = false);

    /*
     * Returns a vector that represents the linear interpolation between this
     * vector and the provided vector. The interpolation value is provided
     * by t [0..1].
     */
    Vector3<Real> linearInterpolation(const Vector3<Real>& vector, Real t);

    /* 
     * Returns a contiguous array pointer to the data contained of this vector.
     * The size of this array is always 3.
     */
    Real* data();
    const Real* const constData() const;
    
    /*
     * Calculates the angle between two vectors. The calculated angle is
     * reported in radians.
     *
     * lhs  rhs
     * |   /
     * |  /
     * |A/
     * |/
     * +
     * Returns the value of A (the smallest angle between the two vectors).
     */
    static double Angle(const Vector3<Real>& u, const Vector3<Real>& v);
    static double Magnitude(const Vector3<Real>& vector);
    static double Length(const Vector3<Real>& vector);
    static double LengthSquared(const Vector3<Real>& vector);

    /*
     * Returns the linearly interpolated vector between vectors a and b. This
     * interpolated vector is based on the interpolation value of t [0..1].
     */
    static Vector3<Real> linearInterpolation(const Vector3<Real>& a, const Vector3<Real>& b, Real t);

    /*
     * Distance between two (n) x (n) vectors u and v:
     * D = sqrt((v1-u1)^2 + (v2-u2)^2 + (v3-u3)^2)
     */
    static double Distance(const Vector3<Real>& u, const Vector3<Real>& v);

    /* 
     * In some instances the squared distance can be used, foregoing the cost
     * of the square root operation. The return value is calcluated by:
     * DS = (v1-u1)^2 + (v2-u2)^2 + (v3-u3)^2
     */
    static double DistanceSquared(const Vector3<Real>& u, const Vector3<Real>& v);

    /* 
     * Calculates the dot product between the two provided vectors. The dot
     * product of two (n) x (n) vectors is as follows:
     * 
     * u.v = Sum[ui * vi] = u1v1 + u2v2 + ... unvn
     */
    static double Dot(const Vector3<Real>& u, const Vector3<Real>& v);

    /* 
     * Calculate the vector perpendicular to the provided u and v
     * vectors. The resulting vector will form the normal of the plane that is
     * formed by vectors u and v (i.e. u and v are coplanar with the plane that
     * they form).
     *
     * Returns the resultant vector that is orthogonal to u and v. 
     * The resulting vector is not normalized.
     * 
     *  u
     *  |   v
     *  |  /
     *  | /
     *  |/
     *  +------- result
     */
    static Vector3<Real> Cross(const Vector3<Real>& u, const Vector3<Real>& v);
    
    /* 
     * Projects the vector u onto the vector v and returns the result. 
     *
     *      (u.v)      -(u.v)
     *        u           u  
     *       /|           |\  
     *      / |           | \
     *     /  |           |  \
     *    /   |           |   \
     *   /    |           |    \
     *  +--r-->-----v     <--r--+-----V
     *
     * The result vector (r) has two forms (u.v) and -(u.v). If u.v
     * is negative then the resulting vector will be pointed in the opposite
     * direction of v. If u.v is positive then r will point in the same 
     * direction as v.
     */
    static Vector3<Real> Project(const Vector3<Real>& u, const Vector3<Real>& v);
    
    /*
     * This static Reflect function calculates the reflection direction of the
     * provided vector based on the normal of the 'surface' that the vector
     * collides with. Both the reflection vector and the normalized reflection
     * direction can be calculated.
     * 
     * vector - The vector to calculate the reflection of.
     * normal - The normal of the plane the vector is colliding with.
     * normalize - Determines if the resulting reflection vector is normalized.
     * backcull - Determines if the vector can 'collide' with the normals plane
     *
     *     Vector
     * |  /
     * | /
     * |/
     * +-------> normal
     * |\
     * | \
     * |  \
     *     Reflection
     *
     * If the vector is pointing in the same direction as the normal, then the
     * reflection vector can either be culled or calculated using the inverse
     * of the normal. This will calculate the reflection vector based on the
     * provided vector colliding with the back-side of the plane formed by the
     * provided normal.
     *
     * If backcull is false then the normal will be inversed and the reflection
     * vector will be valid for the back-side of the plane formed by the normal.
     * If backcull is true then the reflection vector will be set to the input
     * vector.
     */
    static Vector3<Real> Reflect(const Vector3<Real>& vector, const Vector3<Real>& normal, bool normalize = false, bool backcull = false);
    static Vector3<Real> ReflectDir(const Vector3<Real>& vector, const Vector3<Real>& normal, bool backcull = false);

    /* Zero vector defined as: (0, 0, 0) */
    static Vector3<Real> Zero();
    
    /*
     * Constant unit vectors for the primary axes. Each will return a value of
     * 1 in the component of the labeled axis:
     *
     * Unit X (1, 0, 0)
     * Unit Y (0, 1, 0)
     * Unit Z (0, 0, 1)
     */
    static Vector3<Real> UnitX();
    static Vector3<Real> UnitY();
    static Vector3<Real> UnitZ();

    /* Type conversion Operators */
    operator Real* ();
    operator const Real* const () const;

    /* Standard Operators */
    bool operator < (const Vector3<Real>& vector);
    bool operator <= (const Vector3<Real>& vector);
    bool operator > (const Vector3<Real>& vector);
    bool operator >= (const Vector3<Real>& vector);
    
    /* 
     * Exact equality operators. For approximate equality between vectors
     * using epsilon see the function: equivalent(..).
     */
    bool operator == (const Vector3<Real>& vector);
    bool operator != (const Vector3<Real>& vector);

    Vector3<Real>& operator = (const Vector3<Real>& vector);
    Vector3<Real>& operator += (const Vector3<Real>& vector);
    Vector3<Real>& operator -= (const Vector3<Real>& vector);
    Vector3<Real>& operator *= (const Vector3<Real>& vector);
    Vector3<Real>& operator *= (Real scalar);

    /* 
     * Returns the component value at the specified index. The valid
     * indices are 0, 1, and 2. If an index is provided that is < 0 or >= 3
     * then the operator will return a value of 0.
     */
    Real operator [] (int index);
    
    /* Global Vector3 Operators */
    friend Vector3<Real> operator + <> (const Vector3<Real>& lhs, const Vector3<Real>& rhs);
    friend Vector3<Real> operator - <> (const Vector3<Real>& lhs, const Vector3<Real>& rhs);
    friend Vector3<Real> operator - <> (const Vector3<Real>& vector);
    friend Vector3<Real> operator * <> (const Vector3<Real>& vector, Real scalar);
    friend Vector3<Real> operator * <> (Real scalar, const Vector3<Real>& vector);
    friend Vector3<Real> operator / <> (const Vector3<Real>& vector, Real scalar);
    
    /* Gloabl Stream Operators */
    friend std::ostream& operator << <> (std::ostream& out, const Vector3<Real>& vector);
    friend std::istream& operator >> <> (std::istream& in, Vector3<Real>& vector);

protected:
    /* Contiguous set of components: [0] = x; [1] = y; [2] = z; */
    Real tuple[3];
};

/* Vector3 Implementation. */
template <typename Real>
Vector3<Real>::Vector3() {
    this->tuple[X] = static_cast<Real>(0);
    this->tuple[Y] = static_cast<Real>(0);
    this->tuple[Z] = static_cast<Real>(0);
}

template <typename Real>
Vector3<Real>::Vector3(const Vector3<Real>& vector) {
    this->tuple[X] = vector.tuple[X];
    this->tuple[Y] = vector.tuple[Y];
    this->tuple[Z] = vector.tuple[Z];
}

template <typename Real>
Vector3<Real>::Vector3(Real uniform) {
    this->tuple[X] = uniform;
    this->tuple[Y] = uniform;
    this->tuple[Z] = uniform;
}

template <typename Real>
Vector3<Real>::Vector3(Real a[3]) {
    this->tuple[X] = a[0];
    this->tuple[Y] = a[1];
    this->tuple[Z] = a[2];
}

template <typename Real>
Vector3<Real>::Vector3(Real x, Real y, Real z) {
    this->tuple[X] = x;
    this->tuple[Y] = y;
    this->tuple[Z] = z;
}

template <typename Real>
Vector3<Real>::Vector3(const Vector3<Real>& from, const Vector3<Real>& to) {
    Vector3<Real> dir = (to - from).normalized();
    this->tuple[X] = dir.tuple[X];
    this->tuple[Y] = dir.tuple[Y];
    this->tuple[Z] = dir.tuple[Z];
}

template <typename Real>
Vector3<Real>::~Vector3() {}

template <typename Real>
void Vector3<Real>::setX(Real x) {
    this->tuple[X] = x;
}

template <typename Real>
void Vector3<Real>::setY(Real y) {
    this->tuple[Y] = y;
}

template <typename Real>
void Vector3<Real>::setZ(Real z) {
    this->tuple[Z] = z;
}

template <typename Real>
Real Vector3<Real>::getX() const {
    return this->tuple[X];
}

template <typename Real>
Real Vector3<Real>::getY() const {
    return this->tuple[Y];
}

template <typename Real>
Real Vector3<Real>::getZ() const {
    return this->tuple[Z];
}

template <typename Real>
inline Real& Vector3<Real>::x() {
    return this->tuple[X];
}

template <typename Real>
inline Real& Vector3<Real>::y() {
    return this->tuple[Y];
}

template <typename Real>
inline Real& Vector3<Real>::z() {
    return this->tuple[Z];
}

/* Dot product angle between vectors: theta = cos-1(A.B / |A||B|) */
template <typename Real>
double Vector3<Real>::angle(const Vector3<Real>& vector) {
    return std::acos((this->tuple[X] * vector.tuple[X] + this->tuple[Y] * vector.tuple[Y] + this->tuple[Z] * vector.tuple[Z]) / (this->norm() * vector.norm()));
}

template <typename Real>
inline double Vector3<Real>::magnitude() const {
    return std::sqrt(this->lengthSquared());
}

template <typename Real>
inline double Vector3<Real>::norm() const {
    return std::sqrt(this->lengthSquared());
}

template <typename Real>
inline double Vector3<Real>::length() const {
    return std::sqrt(this->lengthSquared());
}

template <typename Real>
inline double Vector3<Real>::lengthSquared() const {
    return std::pow(this->tuple[X], 2.0) + std::pow(this->tuple[Y], 2.0) + std::pow(this->tuple[Z], 2.0);
}

template <typename Real>
inline double Vector3<Real>::distance(const Vector3<Real>& vector) {
    return std::sqrt(this->distanceSquared(vector));
}

template <typename Real>
inline double Vector3<Real>::distanceSquared(const Vector3<Real>& vector) {
    return std::pow(this->tuple[X] - vector.tuple[X], 2.0) + std::pow(this->tuple[Y] - vector.tuple[Y], 2.0) + std::pow(this->tuple[Z] - vector.tuple[Z], 2.0);
}

template <typename Real>
inline void Vector3<Real>::scale(Real scalar) {
    this->tuple[X] *= scalar;
    this->tuple[Y] *= scalar;
    this->tuple[Z] *= scalar;
}

template <typename Real>
inline void Vector3<Real>::multiply(Real scalar) {
    this->tuple[X] *= scalar;
    this->tuple[Y] *= scalar;
    this->tuple[Z] *= scalar;
}

template <typename Real>
inline void Vector3<Real>::multiply(const Vector3<Real>& vector) {
    this->tuple[X] *= vector.tuple[X];
    this->tuple[Y] *= vector.tuple[Y];
    this->tuple[Z] *= vector.tuple[Z];
}

template <typename Real>
inline void Vector3<Real>::add(const Vector3<Real>& vector) {
    this->tuple[X] += vector.tuple[X];
    this->tuple[Y] += vector.tuple[Y];
    this->tuple[Z] += vector.tuple[Z];
}

template <typename Real>
void Vector3<Real>::subtract(const Vector3<Real>& vector) {
    this->tuple[X] -= vector.tuple[X];
    this->tuple[Y] -= vector.tuple[Y];
    this->tuple[Z] -= vector.tuple[Z];
}

template <typename Real>
void Vector3<Real>::normalize() {
    double len = this->length();
    this->tuple[X] /= static_cast<Real>(len);
    this->tuple[Y] /= static_cast<Real>(len);
    this->tuple[Z] /= static_cast<Real>(len);
}

template <typename Real>
void Vector3<Real>::inverse() {
    this->tuple[X] = -this->tuple[X];
    this->tuple[Y] = -this->tuple[Y];
    this->tuple[Z] = -this->tuple[Z];
}

template <typename Real>
void Vector3<Real>::zero() {
    this->tuple[X] = static_cast<Real>(0);
    this->tuple[Y] = static_cast<Real>(0);
    this->tuple[Z] = static_cast<Real>(0);
}

template <typename Real>
bool Vector3<Real>::isZero(Real epsilon) const {
    if ( (this->tuple[X] > -epsilon) && 
         (this->tuple[X] < epsilon ) &&
         (this->tuple[Y] > -epsilon) &&
         (this->tuple[Y] < epsilon ) &&
         (this->tuple[Z] > -epsilon) &&
         (this->tuple[Z] < epsilon ) )
    return true;
    else return false;
}

template <typename Real>
inline bool Vector3<Real>::equal(const Vector3<Real>& vector) {
    if ( *this == vector ) return true;
    return false;
}

template <typename Real>
template <typename RealCastType>
Vector3<RealCastType> Vector3<Real>::cast() {
    return Vector3<RealCastType>(static_cast<RealCastType>(this->tuple[X]), static_cast<RealCastType>(this->tuple[Y]), static_cast<RealCastType>(this->tuple[Z]));
}

template <typename Real>
bool Vector3<Real>::equivalent(const Vector3<Real>& vector, double epsilon) {
    if ( this->tuple[X] > (vector.tuple[X] - epsilon) &&
         this->tuple[X] < (vector.tuple[X] + epsilon) &&
         this->tuple[Y] > (vector.tuple[Y] - epsilon) &&
         this->tuple[Y] < (vector.tuple[Y] + epsilon) &&
         this->tuple[Z] > (vector.tuple[Z] - epsilon) &&
         this->tuple[Z] < (vector.tuple[Z] + epsilon) ) return true;
    else return false;
}

template <typename Real>
void Vector3<Real>::set(const Vector3<Real>& data) {
    this->tuple[X] = data[X];
    this->tuple[Y] = data[Y];
    this->tuple[Z] = data[Z];
}

template <typename Real>
void Vector3<Real>::set(Real x, Real y, Real z) {
    this->tuple[X] = x;
    this->tuple[Y] = y;
    this->tuple[Z] = z;
}

template <typename Real>
void Vector3<Real>::set(Axis axis, Real& value) {
    this->tuple[axis] = value;
}

template <typename Real>
void Vector3<Real>::setLength(Real length) {
    Real len = this->length();

    if ( len != static_cast<Real>(0) ) {
        Real factor = length / len;
        this->tuple[0] *= factor;
        this->tuple[1] *= factor;
        this->tuple[2] *= factor;
    }
}

template <typename Real>
void Vector3<Real>::get(Vector3<Real>& data) const {
    data.tuple[X] = this->tuple[X];
    data.tuple[Y] = this->tuple[Y];
    data.tuple[Z] = this->tuple[Z];
}

template <typename Real>
void Vector3<Real>::get(Real& x, Real& y, Real& z) const {
    x = this->tuple[X];
    y = this->tuple[Y];
    z = this->tuple[Z];
}

template <typename Real>
void Vector3<Real>::get(Axis axis, Real& value) const {
    value = this->tuple[axis];
}

template <typename Real>
Real Vector3<Real>::get(Axis axis) const {
    return this->tuple[axis];
}

template <typename Real>
inline double Vector3<Real>::dot(const Vector3<Real>& vector) const {
    return Vector3<Real>::Dot(*this, vector);
}

template <typename Real>
inline double Vector3<Real>::innerProduct(const Vector3<Real>& vector) const {
    return this->dot(vector);
}

template <typename Real>
Vector3<Real> Vector3<Real>::cross(const Vector3<Real>& vector) const {
    return Vector3<Real>::Cross(*this, vector);
}

template <typename Real>
Vector3<Real> Vector3<Real>::normalized() const {
    Vector3<Real> result(*this);
    result.normalize();
    return result;
}

template <typename Real>
Vector3<Real> Vector3<Real>::reflect(const Vector3<Real>& normal, bool backcull) {
    return Vector3<Real>::Reflect(*this, normal, false, backcull);
}

template <typename Real>
Vector3<Real> Vector3<Real>::reflectDir(const Vector3<Real>& normal, bool backcull) {
    return Vector3<Real>::Reflect(*this, normal, true, backcull);
}

template <typename Real>
Vector3<Real> Vector3<Real>::linearInterpolation(const Vector3<Real>& vector, Real t) {
    return Vector3<Real>::linearInterpolation(*this, vector, t);
}

template <typename Real>
inline Real* Vector3<Real>::data() {
    return this->tuple;
}

template <typename Real>
inline const Real* const Vector3<Real>::constData() const {
    return this->tuple;
}

template <typename Real>
double Vector3<Real>::Angle(const Vector3<Real>& u, const Vector3<Real>& v) {
    return std::acos((u.tuple[X] * v.tuple[X] + u.tuple[Y] * v.tuple[Y] + u.tuple[Z] * v.tuple[Z]) / (u.norm() * v.norm()));
}

template <typename Real>
double Vector3<Real>::Magnitude(const Vector3<Real>& vector) {
    return std::sqrt(Vector3<Real>::LengthSquared(vector));
}

template <typename Real>
double Vector3<Real>::Length(const Vector3<Real>& vector) {
    return std::sqrt(Vector3<Real>::LengthSquared(vector));
}

template <typename Real>
double Vector3<Real>::LengthSquared(const Vector3<Real>& vector) {
    return std::pow(vector.tuple[X], 2.0) + std::pow(vector.tuple[Y], 2.0) + std::pow(vector.tuple[Z], 2.0);
}

template <typename Real>
Vector3<Real> Vector3<Real>::linearInterpolation(const Vector3<Real>& a, const Vector3<Real>& b, Real t) {
    return (a * t) + b * (static_cast<Real>(1) - t);
}

template <typename Real>
inline double Vector3<Real>::Distance(const Vector3<Real>& u, const Vector3<Real>& v) {
    return std::sqrt(Vector3<Real>::DistanceSquared(u, v));
}

template <typename Real>
inline double Vector3<Real>::DistanceSquared(const Vector3<Real>& u, const Vector3<Real>& v) {
    return std::pow(u.tuple[X] - v.tuple[X], 2.0) + std::pow(u.tuple[Y] - v.tuple[Y], 2.0) + std::pow(u.tuple[Z] - v.tuple[Z], 2.0);
}

/* (a . b) = Sum_n ai*bi = a1b1 + a2b2 + ... + anbn */
template <typename Real>
inline double Vector3<Real>::Dot(const Vector3<Real>& u, const Vector3<Real>& v) {
    return (u.tuple[X] * v.tuple[X]) + (u.tuple[Y] * v.tuple[Y]) + (u.tuple[Z] * v.tuple[Z]);
}

/*
 * Formal Determinant Formulation (i, j, k represent ortho axes):
 * 
 *         | i  j  k  |
 * u x v = | u1 u2 u3 |
 *         | v1 v2 v3 |
 *
 * Cofactors: 
 * i | u2 u3 |
 *   | v2 v3 |
 *
 * j | u1 u3 |
 *   | v1 v3 |
 *
 * k | u1 u2 |
 *   | v1 v2 |
 * 
 * Determinants:
 * x =  (u2 * v3 - u3 * v2)i
 * y = -(u1 * v3 - u3 * v1)j
 * z =  (u1 * v2 - u2 * v1)k
 *
 * Where 1 = x, 2 = y, and 3 = z
 */
template <typename Real>
Vector3<Real> Vector3<Real>::Cross(const Vector3<Real>& u, const Vector3<Real>& v) {
    Vector3<Real> result;
    result.tuple[X] = ((u.tuple[Y] * v.tuple[Z]) - (u.tuple[Z] * v.tuple[Y]));
    result.tuple[Y] = -((u.tuple[X] * v.tuple[Z]) - (u.tuple[Z] * v.tuple[X]));
    result.tuple[Z] = ((u.tuple[X] * v.tuple[Y]) - (u.tuple[Y] * v.tuple[X]));
    return result;
}

/* 
 * Returns u projected onto v.
 * A negative dot product will inverse the direction of the projection.
 */
template <typename Real>
Vector3<Real> Vector3<Real>::Project(const Vector3<Real>& u, const Vector3<Real>& v) {
    double dot = u.dot(v) / v.dot(v);
    return v * dot;
}

/*
 * Provides a reflect implementation between a vector and an infinite plane
 * defined only be the provided normal. If the vector is pointing in the
 * same direction as the plane normal then this function will inverse the
 * normal of the plane if backcull is set to false (if true the collision
 * with the back-side of the plane will be culled). If normalize is set to
 * true then the resulting reflection vector direction will be returned.
 *
 */
template <typename Real>
Vector3<Real> Vector3<Real>::Reflect(const Vector3<Real>& vector, const Vector3<Real>& normal, bool normalize, bool backcull) {
    Vector3<Real> dir = vector.normalized();
    Vector3<Real> n = normal;
    Vector3<Real> a = Vector3<Real>::Zero();

    double dot = n.dot(dir);
    double mag = vector.norm();
    
    //--------------------------------------------------------------------------
    // If the incoming vector is completely orthogonal to the normal, then
    // reflect the vector back in the direction it came.
    //--------------------------------------------------------------------------
    if ( dot == static_cast<Real>(0) ) {
        return -vector;
    }
    //--------------------------------------------------------------------------
    // The vector will hit the plane defined by the normal, since this is the
    // case, the reflection vector will have to be calculated.
    //--------------------------------------------------------------------------
    else if ( dot < static_cast<Real>(0) ) {
        a = n * static_cast<Real>(dot) * static_cast<Real>(-1);
    }
    //--------------------------------------------------------------------------
    // The vector will collide with the plane of the provided normal.
    // If backcull is defined as true then the incoming vector will be ignored
    // and allowed to pass through the plane defined by the provided normal.
    // If backcull is defined as false, then the vector will collide with the
    // back-side of the plane. To achieve this the normal of the plane is
    // inversed.
    //--------------------------------------------------------------------------
    else {
        if ( backcull == false ) {
            n *= static_cast<Real>(-1);
            a = n * static_cast<Real>(dot);
        }
        else return vector;
    }

    //--------------------------------------------------------------------------
    // Calculate the reflection vector (either the normalized direction or
    // restore the incoming magnitude).
    //--------------------------------------------------------------------------
    Vector3<Real> twoA = (static_cast<Real>(2) * a);
    Vector3<Real> reflectionDir = (dir + twoA).normalized();
    if ( normalize == true ) return reflectionDir;
    Vector3<Real> reflected = reflectionDir * static_cast<Real>(mag);
    return reflected;
}

template <typename Real>
Vector3<Real> Vector3<Real>::ReflectDir(const Vector3<Real>& vector, const Vector3<Real>& normal, bool backcull) {
    return Vector3<Real>::Reflect(vector, normal, true, backcull);
}

template <typename Real>
inline Vector3<Real> Vector3<Real>::Zero() {
    return Vector3<Real>();
}

template <typename Real>
inline Vector3<Real> Vector3<Real>::UnitX() {
    return Vector3<Real>(static_cast<Real>(1), static_cast<Real>(0), static_cast<Real>(0));
}

template <typename Real>
inline Vector3<Real> Vector3<Real>::UnitY() {
    return Vector3<Real>(static_cast<Real>(0), static_cast<Real>(1), static_cast<Real>(0));
}

template <typename Real>
inline Vector3<Real> Vector3<Real>::UnitZ() {
    return Vector3<Real>(static_cast<Real>(0), static_cast<Real>(0), static_cast<Real>(1));
}

template <typename Real>
inline Vector3<Real>::operator Real* () {
    return this->tuple;
}

template <typename Real>
inline Vector3<Real>::operator const Real* const () const {
    return this->tuple;
}

template <typename Real>
bool Vector3<Real>::operator < (const Vector3<Real>& vector) {
    if ( this->norm() < vector.norm() ) return true;
    return false;
}

template <typename Real>
bool Vector3<Real>::operator <= (const Vector3<Real>& vector) {
    if ( this->norm() <= vector.norm() ) return true;
    return false;
}

template <typename Real>
bool Vector3<Real>::operator > (const Vector3<Real>& vector) {
    if ( *this->norm() > vector.norm() ) return true;
    return false;
}

template <typename Real>
bool Vector3<Real>::operator >= (const Vector3<Real>& vector) {
    if ( this->norm() >= vector.norm() ) return true;
    return false;
}

template <typename Real>
bool Vector3<Real>::operator == (const Vector3<Real>& vector) {
    if ( this->tuple[X] == vector.tuple[X] && this->tuple[Y] == vector.tuple[Y] && this->tuple[Z] == vector.tuple[Z] ) return true;
    return false;
}

template <typename Real>
bool Vector3<Real>::operator != (const Vector3<Real>& vector) {
    return !(*this == vector);
}

template <typename Real>
Vector3<Real>& Vector3<Real>::operator = (const Vector3<Real>& vector) {
    if ( this == &vector ) return *this;
    this->tuple[X] = vector.tuple[X];
    this->tuple[Y] = vector.tuple[Y];
    this->tuple[Z] = vector.tuple[Z];
    return *this;
}

template <typename Real>
Vector3<Real>& Vector3<Real>::operator += (const Vector3<Real>& vector) {
    this->tuple[X] += vector.tuple[X];
    this->tuple[Y] += vector.tuple[Y];
    this->tuple[Z] += vector.tuple[Z];
    return *this;
}

template <typename Real>
Vector3<Real>& Vector3<Real>::operator -= (const Vector3<Real>& vector) {
    this->tuple[X] -= vector.tuple[X];
    this->tuple[Y] -= vector.tuple[Y];
    this->tuple[Z] -= vector.tuple[Z];
    return *this;
}

template <typename Real>
Vector3<Real>& Vector3<Real>::operator *= (const Vector3<Real>& vector) {
    this->tuple[X] *= vector.tuple[X];
    this->tuple[Y] *= vector.tuple[Y];
    this->tuple[Z] *= vector.tuple[Z];
    return *this;
}

template <typename Real>
Vector3<Real>& Vector3<Real>::operator *= (Real scalar) {
    this->tuple[X] *= scalar;
    this->tuple[Y] *= scalar;
    this->tuple[Z] *= scalar;
    return *this;
}

template <typename Real>
Real Vector3<Real>::operator [] (int index) {
    if ( index < 0 || index >= 3 ) return static_cast<Real>(0);
    return this->tuple[index];
}

template <typename Real>
Vector3<Real> operator + (const Vector3<Real>& lhs, const Vector3<Real>& rhs) {
    return Vector3<Real>(lhs.tuple[X] + rhs.tuple[X], lhs.tuple[Y] + rhs.tuple[Y], lhs.tuple[Z] + rhs.tuple[Z]);
}

template <typename Real>
Vector3<Real> operator - (const Vector3<Real>& lhs, const Vector3<Real>& rhs) {
    return Vector3<Real>(lhs.tuple[X] - rhs.tuple[X], lhs.tuple[Y] - rhs.tuple[Y], lhs.tuple[Z] - rhs.tuple[Z]);
}

template <typename Real>
Vector3<Real> operator - (const Vector3<Real>& vector) {
    return Vector3<Real>(vector.tuple[X] * static_cast<Real>(-1), vector.tuple[Y] * static_cast<Real>(-1), vector.tuple[Z] * static_cast<Real>(-1));
}

template <typename Real>
Vector3<Real> operator * (const Vector3<Real>& vector, Real scalar) {
    return Vector3<Real>(vector.tuple[X] * scalar, vector.tuple[Y] * scalar, vector.tuple[Z] * scalar);
}

template <typename Real>
Vector3<Real> operator * (Real scalar, const Vector3<Real>& vector) {
    return Vector3<Real>(vector.tuple[X] * scalar, vector.tuple[Y] * scalar, vector.tuple[Z] * scalar);
}

template <typename Real>
Vector3<Real> operator / (const Vector3<Real>& vector, Real scalar) {
    Real invScalar = static_cast<Real>(1) / scalar;
    return Vector3<Real>(vector.tuple[X] * invScalar, vector.tuple[Y] * invScalar, vector.tuple[Z] * invScalar);
}

template <typename Real>
std::ostream& operator << (std::ostream& out, const Vector3<Real>& vector) {
    out << vector.tuple[X] << Vector3<Real>::SEPARATOR << vector.tuple[Y] << Vector3<Real>::SEPARATOR << vector.tuple[Z];
    return out;
}

template <typename Real>
std::istream& operator >> (std::istream& in, Vector3<Real>& vector) {
    in >> vector.tuple[Vector3<Real>::X];
    in >> vector.tuple[Vector3<Real>::Y];
    in >> vector.tuple[Vector3<Real>::Z];
    return in;
}

typedef Vector3<char> Vector3b;
typedef Vector3<float> Vector3f;
typedef Vector3<double> Vector3d;
typedef Vector3<short> Vector3s;
typedef Vector3<int> Vector3i;

}

}

#endif
